/* ------------------------- Port definitions ------------------------- */ 
/* Motor ports */ 
#define left_motor1 2 
#define left_motor2 3 
#define right_motor1 0 
#define right_motor2 1 

#define baby_motor1 4
#define baby_motor2 5

/* Shaft encoder ports */
/* 7 dint work - now it does */ 
#define left_shaft 7
#define right_shaft 8

/* Sensor ports */ 
/* Analog */
#define a_sensor0 2
#define a_sensor1 3
#define a_sensor2 4
#define a_sensor3 5

/* Digital */
#define d_sensor_front1 12
#define d_sensor_front2 13

 /* these sensors are on the back of the robot on the front wheel */
#define d_sensor_back1 14 /* left */
#define d_sensor_back2 15 /* right */

#define start_sensor 16

/* Servo port */ 
#define arm_servo 0

/* ------------------------- Other constants ------------------------- */
/* colors and directions */
#define black 0
#define white 1

#define north 0
#define east 1
#define west 2
#define south 3

/* Sensor variables */
#define invalid 9
int side = invalid;
 
/* Servo orientations  
*/
#define arm_up 3000
#define arm_down 100
#define arm_servo_ini arm_up

/* Shaft definitions */
#define max_turntime 10000.0
#define shafts_straight 100

#define l_shaft_lturn90 9 /*9  25 85 */
#define r_shaft_lturn90 10 /*11 10 20  145 */

#define l_shaft_rturn90 10 /*10 12 12 12 8 7 145 */
#define r_shaft_rturn90 9 /*10 9 11 9 13 85 */

#define l_shaft_r180 26
#define r_shaft_r180 20 /*30 31 */

#define l_shaft_l180 20 /* 23 17 18 */ 
#define r_shaft_l180 21 /*23 27 */

#define l_shaft_hlturn 5
#define r_shaft_hlturn 5
#define l_shaft_hrturn 6
#define r_shaft_hrturn 5

#define l_st_2 62
#define r_st_2 72

#define reduced_speed 100
#define threshold 0

/* Miscellaneous functions */ 

int black_threshold(int p)
{
  if (p == a_sensor0) return 40;
  else if (p == a_sensor1) return 90;
  else if (p == a_sensor2) return 23;
  else if (p == a_sensor3) return 40;

/* initial threshold values for 0,1,2,3 are 40,90,23,40 */
}

int white_threshold(int p)
{
  if (p == a_sensor0) return 15;
  else if (p == a_sensor1) return 33;
  else if (p == a_sensor2) return 15;
  else if (p == a_sensor3) return 20;
/* initial threshold values for 0,1,2,3 are 15,33,15,20 */
}

void starter(char s[]) 
{ 
  printf("%s \n", s); 
  wait_start(); 
} 

void p(char s[]) 
{ 
  printf("%s \n", s); 
} 

void wait_start() 
{ 
  while (!press_start()); 
} 

int press_start() 
{ 
  if (!start_button()) return 0; 
  while (start_button()); 
  return 1; 
} 

int press_stop() 
{ 
  if (!stop_button()) return 0; 
  while (stop_button()); 
  return 1; 
} 

void hold_up()
{
  sleep(0.0001);
}


/* ------------------------- Handlers ------------------------- */ 
/* shaft stuff */
void shaft_handler() 
{ 
  reset_encoder(left_shaft); 
  reset_encoder(right_shaft); 
} 
void enable_encoders()
{
  enable_encoder(left_shaft);
  enable_encoder(right_shaft);
}
void reset_encoders() 
{ 
  reset_encoder(left_shaft); 
  reset_encoder(right_shaft); 
} 
void disable_encoders()
{
  disable_encoder(left_shaft);
  disable_encoder(right_shaft);
}

/* servo stuff */
void start_servos() 
{ 
  enable_servos(); 
  servo (arm_servo, arm_up); 
  printf("Servo oriented at %d", arm_servo_ini); 
} 
void move_arm(int p) 
{ 
  servo(arm_servo, p); 
} 

/* ------------------------- motor functions ------------------------- */ 
/* Run motors with specified powers - the wrong motor has been accounted for*/ 
void run_motors(int a, int b, int c, int d) 
{ 
  motor (left_motor1, a); 
  motor (left_motor2, b); 
  motor (right_motor1, c); 
  motor (right_motor2, d); 
} 

/* Run motors at full speed */ 
void forw() 
{ 
  fd(left_motor1); 
  fd(left_motor2); 
  fd(right_motor1); 
  fd(right_motor2); 
} 

void left() 
{ 
  bk(left_motor1); 
  bk(left_motor2); 
  fd(right_motor1); 
  fd(right_motor2);
} 

void right() 
{ 
  fd(left_motor1); 
  fd(left_motor2); 
  bk(right_motor1); 
  bk(right_motor2); 
} 

void stop() 
{ 
  ao(); 
} 

void reverse()
{
  bk(left_motor1);
  bk(left_motor2);
  bk(right_motor1);
  bk(right_motor2);
}
void reverse_left()
{
  bk(left_motor1);
  bk(left_motor2);
  motor(right_motor1, - 10);
  motor(right_motor2, - 10);
}

void reverse_right()
{
  motor (left_motor1, - 10);
  motor(left_motor2, - 10);
  bk(right_motor1);
  bk(right_motor2);
}

int check_range(int l, int r)
{
  int left_low, left_high;
  int right_low, right_high;
  left_low = l - threshold;
  right_low = r - threshold;

  if (l == r) 
    return 0;
  else
    if (l < r)
      {
	if (l > right_low)
	  return 0;
	else return -1;
      }
    else
      {
	if (r > left_low)
	  return 0;
	else return 1;
      }
}

/* digital sensors */
/* orientation Sensor stuff */
int check_color(int port) 
{ 
  int reading = analog(port); 
  if (reading > black_threshold(port)) 
    return 0; 
  if (reading < white_threshold(port)) 
    return 1; 
  else return invalid; 
} 

/* starting */
void startup()
{
  ir_transmit_off();
  start_machine(start_sensor);
  ir_transmit_on();
  reset_system_time();
  printf("starting...");

}



/* ------------------------- Movements ------------------------- */ 

void straight(int i) 
{ 
  int l, r;
  int c, mspeed = 100;
  int sensleft, sensright, senscount;
  float t, timenow;
  t = seconds();
  p("Going straight"); 
  shaft_handler();
  l = read_encoder(left_shaft);
  r = read_encoder(right_shaft);
  if (i == 1)
    {
      run_motors(100, 100, 90, 90);
      while ((l <= 55) &&
	     (r <= 55))
	{
	  l = read_encoder(left_shaft);
	  r = read_encoder(right_shaft);
	  hold_up();
	}   
      run_motors(70,70,60,60);
      sleep(0.1);
      run_motors(50,50,40,40);
      sleep(0.1);
      run_motors(30,30,20,20);
      sleep(0.1);
      ao();
    }
  else
    if (i==2)
    {
      run_motors(100, 100, 90, 90);
      sleep(1.0);
      ao();
    }
    else
      if (i==3)
	{
	  if (side==white)
	    {
	      run_motors(0, 0, -100, -100);
	      while (r <= 55)
		{
		  l = read_encoder(left_shaft);
		  r = read_encoder(right_shaft);
		}
	    }
	  if (side==black)
	    {
	      run_motors(-100, -100, 0, 0);
	      while (l <= 55)
		{
		  l = read_encoder(left_shaft);
		  r = read_encoder(right_shaft);
		}
	    }
	  
	}
      else 
	if
	(i == 4)
	  {
	    run_motors(-100,-100, -90, -90);
	    while ((l <= 40) &&
		   (r <= 40))
	      {
		l = read_encoder(left_shaft);
		r = read_encoder(right_shaft);
	      }
	    
	  }
} 

void turner(int sp1, int sp2, int shaf1, int shaf2)
{
  int l, r;
  shaft_handler();
  l = read_encoder(left_shaft);
  r = read_encoder(right_shaft);
  run_motors(sp1, sp1, sp2, sp2);
  while ((l <= shaf1) ||
	(r <= shaf2))
    {
      l = read_encoder(left_shaft);
      r = read_encoder(right_shaft);
      if (l >= shaf1)
	{
	  motor(left_motor1, 0);
	  motor(left_motor2, 0); 
	}
      if (r >= shaf2)
	{
	  motor(right_motor1, 0);
	  motor(right_motor2, 0);
	}
    }
  ao();
}

void trun(int sp1, int sp2, int shaf1, int shaf2)
{
  int l, r;
  printf("Doing trun");
  shaft_handler();
  l = read_encoder(left_shaft);
  r = read_encoder(right_shaft);
  run_motors(sp1, sp1, sp2, sp2);
  if (shaf1 == 0)
    {
      while (r <= shaf2)
	{
	  l = read_encoder(left_shaft);
	  r = read_encoder(right_shaft);
	  if (l >= shaf1)
	    {
	      motor(left_motor1, 0);
	      motor(left_motor2, 0);
	    }
	  if (r >= shaf2)
	    {
	      motor(right_motor1, 0);
	      motor(right_motor2, 0);
	    }}}
  else
    {
      while (l <= shaf1)
	{
	  l = read_encoder(left_shaft);
	  r = read_encoder(right_shaft);
	  if (l >= shaf1)
	    {
	      motor(left_motor1, 0);
	      motor(left_motor2, 0);
	    }
	  if (r >= shaf2)
	    {
	      motor(right_motor1, 0);
	      motor(right_motor2, 0);
	    }}}
}
     
void turn_left() 
{ 
  turner(-100, 100, l_shaft_lturn90, r_shaft_lturn90);
} 

void turn_half(int i)
{
  if (i == 1)
    {
      if (side == black)
	{
	  turner(80, -80, 7, 7);
	}
      else 
	{
	  turner(-80, 80, 7, 7);
	}
    }
  else 
    {
      if (side == black)
	{
	  turner(80, -80, 5, 5);
	}
      else 
	{
	  turner(-80, 80, 5, 5);
	}
    }
  ao();
}

void turn_right() 
{ 
  turner(100, -100, l_shaft_rturn90, r_shaft_rturn90);
} 

void turn_180_left()
{
  turner(-100, 100, l_shaft_l180, r_shaft_l180);
}

void backup()
{
  int l, r;
  int sl, sr;
  float t = seconds();
  printf("Doing backup \n");
  shaft_handler();
  sl = read_encoder(left_shaft);
  sr = read_encoder(right_shaft);

  l = digital(d_sensor_back1);
  r = digital(d_sensor_back2);
  if (side == white)
    {
      printf("inside white backup\n");
      trun(100, 0, 20, 0); 
      l = digital(d_sensor_back1);
      r = digital(d_sensor_back2);

      printf("Back start \n");
      reverse();
      sleep(0.2);
      while ((l == 0) ||
	     (r == 0))
	{
	  printf("gone in");
	  if (l == 1)
	    {
	      if (r == 0) stop();
	      while (r == 0)
		{ reverse_right(); 
		r = digital(d_sensor_back2);}
	      break;
	    }
	  if (r == 1)
	    {
	      if (l == 0) stop();
	      while (l != 1)
		{reverse_left();
	         l = digital(d_sensor_back1);
	       }
	      break;
	    }
	  reverse();
	  hold_up();
	  l = digital(d_sensor_back1);
	  r = digital(d_sensor_back2);
	  
	  if ((seconds() - t) > 5.0)
	    { printf("breaking too long"); 
	      break;}
	}
    } 
  else
    {
      printf("inside black backup \n");
      trun(0, 100, 0, 20);
      l = digital(d_sensor_back1);
      r = digital(d_sensor_back2);
      printf("Back start \n");
      reverse();
      sleep(0.2);
      while ((l == 0) ||
	     (r == 0))
	{
	  printf("gone in");
	  if (l == 1)
	    {
	      if (r == 0) stop();
	      while (r == 0)
		{ reverse_right(); 
		r = digital(d_sensor_back2);}
	      break;
	    }
	  if (r == 1)
	    {
	      if (l == 0) stop();
	      while (l != 1)
		{reverse_left();
	         l = digital(d_sensor_back1);
	       }
	      break;
	    }
	  reverse();
	  hold_up();
	  l = digital(d_sensor_back1);
	  r = digital(d_sensor_back2);
	  
	  if ((seconds() - t) > 5.0)
	    {printf("breaking too long");
	     break;}
	}	      
    } 
  printf("back end");
  ao();
} 

void capture_ball()
{
  move_arm(arm_down);
  printf("doing servo down");
  sleep(0.2);
}

void shoot_ball()
{
  move_arm(arm_up);
  sleep(0.01);
  printf("Running motors");
  run_motors(100, 100, 90, 90);
  sleep(0.4);
  ao();
  sleep(0.2);
  printf("Now backwardS");
  run_motors(-100, -100, -90, -90);
  sleep(0.4);
  ao();
}

void frontup()
{
  float t = seconds();
  straight(3);
}

void give_birth()
{
  fd(baby_motor1);
  fd(baby_motor2);
}





/* 
function for orientation

 0           1 
 ------------- 
       N 

W            E 

  
       S 
- -------------- 
2            3 

 */
             
void orient_sensor()
{
  int i = 1;
  int l, r;
  int shaftvalue = 29;
  int counter = 0;
  int s0 = check_color(a_sensor0);
  int s1 = check_color(a_sensor1); 
  int s2 = check_color(a_sensor2);
  int s3 = check_color(a_sensor3);   

  shaft_handler(); /* new */
  l = read_encoder(left_shaft);
  r = read_encoder(right_shaft);
  printf("l=%d, r=%d \n", l, r);
  
  while (i)
    {
      if ((s0 == 0) &&
	  (s1 == 0) &&
	  (s2 == 0) &&
	  (s3 == 1))
	{
	  side = black;
	  i = 0;
	  turn_left();
	  printf("Turned left...");
	  shaftvalue = 22;
	}
      else
	if ((s0 == 0) &&
	    (s1 == 0) &&
	    (s2 == 1) &&
	    (s3 == 0))
	  {
	    i = 0;
	    side = black;
	    /* oriented */
	    printf("Oriented ...");
	    shaftvalue = 19;
	  }
	else 
	  if ((s0 == 0) &&
	      (s1 == 1) &&
	      (s2 == 0) &&
	      (s3 == 0))
	    {
	      i = 0;
	      side = black;
	      turn_180_left();
	      printf("Turned 180...");
	      shaftvalue = 24;
	    }
	  else
	    if ((s0 == 1) &&
		(s1 == 0) &&
		(s2 == 0) &&
		(s3 == 0))
	      {
		i = 0;
		side = black;
		turn_right();
		printf("Turned right...");
		shaftvalue = 22;
	      }
	    else 
	      if ((s0 == 1) &&
		  (s1 == 1) &&
		  (s2 == 1) &&
		  (s3 == 0))
		{
		  i = 0;
		  side = white;
		  printf("Oriented ...");
		  /* oriented */
		  shaftvalue = 19;
		}
	      else
		if ((s0 == 1) &&
		    (s1 == 1) &&
		    (s2 == 0) &&
		    (s3 == 1))
		  {
		    i = 0;
		    side = white;
		    turn_right();
		    printf("Turned right...");
		    shaftvalue = 23;
		  }
		else 
		  if ((s0 == 1) &&
		      (s1 == 0) &&
		      (s2 == 1) &&
		      (s3 == 1))
		    {
		      i = 0;
		      side = white;
		      turn_left();
		      printf("Turned left...");
		      shaftvalue = 23;
		    }
		  else 
		    if ((s0 == 0) &&
			(s1 == 1) &&
			(s2 == 1) &&
			(s3 == 1))
		      {
			i = 0;
			side = white;
			turn_180_left();
			printf("Turned 180...");
			shaftvalue = 23;
		      }
      
      s0 = check_color(a_sensor0);
      s1 = check_color(a_sensor1); 
      s2 = check_color(a_sensor2);
      s3 = check_color(a_sensor3); 
      if (counter > 10) break;
      counter++;
    }
  shaft_handler(); /* new */
  printf("side=%d sensors%d,%d,%d,%d \n", side, s0, s1, s2, s3);
  if (counter > 10)
    {
      p("couldnt find ... trying random");
      side = black;
      turn_left();
    }
/*  shaftvalue = shaftvalue + 6; for qualification*/
  while ((l < shaftvalue) ||
	(r < shaftvalue))
    {
      run_motors(100,100,90,90);
      l = read_encoder(left_shaft);
      r = read_encoder(right_shaft);
      printf("l=%d, r=%d \n", l, r);
    }
  stop();
}


void main()
{
  int i;
  startup();
  enable_encoders();  
  start_servos();

  orient_sensor();
  capture_ball();
  move_arm(400);
  sleep(0.2);
  backup();
  straight(1);
  move_arm (500);
  turn_half(1);
  sleep(0.2);
  printf("starting shoot \n");
  shoot_ball();
  turn_half(2);
  straight(2);
  give_birth();
  
  i = 0;
  while (seconds() < 60.0)
    {
      hold_up();
      if ((seconds() > 45.0) &&
	  (seconds() < 47.0) &&
	  (i < 3))
	{
	  give_birth();
	  i ++ ;
	}
      if ((seconds() > 50.0) &&
	  (seconds() < 54.0) &&
	  (i < 3))
	{
	  give_birth();
	  i ++;
	}
    }
  disable_servos();
  ir_transmit_off();
}